#include "fmm.h"
#include "isoline.h"

bool draw_one_isoline(Mesh & mesh, double isovalue, std::vector<Mesh::Point> & v_result)
{
	// this vector stores the isolines
	std::vector<Mesh::Point>::iterator cog_it;


	// smoothing mesh argv[1] times
	Mesh::VertexIter          v_it, v_end(mesh.vertices_end());
	Mesh::VertexVertexIter    vv_it;
	Mesh::Point               cog;


	Mesh::FaceIter   f_it, f_end(mesh.faces_end());
	Mesh::FaceVertexIter fv_it,fv_it_end;


	int c = 0;
	/*
	for (v_it=mesh.vertices_begin(); v_it!=v_end; ++v_it)
	{
		cog = mesh.point(v_it);
		//double d = v_it->get_distance();
		mesh.data(v_it).distance = std::sqrt (cog[0] * cog[0] + cog[1]* cog[1] + cog[2]*cog[2]);
		std::cout<<c++<<':'<< cog[0]<<' '<<cog[1]<< ' '<<cog[2]<<' '<<mesh.data(v_it).distance <<std::endl;
	}
	*/

	for (f_it = mesh.faces_begin(); f_it!=f_end; ++f_it)
	{
		fv_it = mesh.fv_iter(f_it);
		fv_it_end = fv_it;
		int count = 0;
		Mesh::Point triangle[3];
		double dis [3];
		do
		{
			cog = mesh.point(fv_it);
			//std::cout<<'('<<cog[0]<<' '<<cog[1]<< ' '<<cog[2]<<')';
			triangle[count][0] = cog[0]; triangle[count][1] = cog[1]; triangle[count][2] = cog[2];
			dis [count] = mesh.data(fv_it).get_distance(); 
			if (++count >= 3) break;
			++fv_it;
		}while (fv_it !=fv_it_end);
		//std::cout<<std::endl;

		/*
		std::cout<<"Triangle :"<<std::endl;
		for (int k = 0; k < 3; ++k)
		{
			std::cout<<triangle[k][0]<<' '<<triangle[k][1]<<' '<<triangle[k][2]<<' '<<dis[k]<<std::endl;
		}
		*/
		Mesh::Point p;
		count = 0;
		Mesh::Point linep[3];

		if (fabs (isovalue - dis[0]) < 1e-7)
		{
			linep[0] = triangle [0];
			count ++;
			if (on_edge (triangle[1], triangle[2], dis[1], dis[2], isovalue, p ))
			{
				linep[1] = p; count++;
			}
		}
		else if (fabs (isovalue - dis[1]) < 1e-7)
		{
			linep[0] = triangle [1];
			count ++;
			if (on_edge (triangle[0], triangle[2], dis[0], dis[2], isovalue, p ))
			{
				linep[1] = p; count++;
			}
		} 
		else if (fabs (isovalue - dis[2]) < 1e-7)
		{
			linep[0] = triangle [2];
			count ++;
			if (on_edge (triangle[1], triangle[0], dis[1], dis[0], isovalue, p ))
			{
				linep[1] = p; count++;
			}
		} 
		else
		{
			if (on_edge (triangle[0], triangle[1], dis[0], dis[1], isovalue, p ))	  
			{
				linep[count][0] = p[0];linep[count][1] = p[1];linep[count][2] = p[2];
				count ++;
			}
			if (on_edge (triangle[1], triangle[2], dis[1], dis[2], isovalue, p ))	  
			{
				linep[count][0] = p[0];linep[count][1] = p[1];linep[count][2] = p[2];
				count ++;
			}
			if (on_edge (triangle[2], triangle[0], dis[2], dis[0], isovalue, p ))	  
			{
				linep[count][0] = p[0];linep[count][1] = p[1];linep[count][2] = p[2];
				count ++;
			}
		}
		if (count == 2)
		{
			v_result.push_back(linep[0]); 
			v_result.push_back(linep[1]);
		}
	}
	return true;
}
bool on_edge (Mesh::Point & p1, Mesh::Point & p2, double value1, double value2, double isovalue, Mesh::Point & p)
{
	//std::cout<<value1 << ' '<<value2<<std::endl;
	/*
	if (std::abs (isovalue - value1) < 1e-7) 
	{
	p[0] = p1[0]; p[1] = p1[1]; p[2] = p1[2];
	return true;
	}
	if (std::abs (isovalue - value2) < 1e-7) 
	{
	p[0] = p2[0]; p[1] = p2[1]; p[2] = p2[2];
	return true;
	}
	*/
	if (isovalue > value1 && isovalue < value2)
	{
		p[0] = float(p1[0] + (p2[0] - p1[0])* (isovalue - value1)/ (value2-value1)); 
		p[1] = float(p1[1] + (p2[1] - p1[1])* (isovalue - value1)/ (value2-value1));
		p[2] = float(p1[2] + (p2[2] - p1[2])* (isovalue - value1)/ (value2-value1));
		//		std::cout<<"Cut1 : "<< p[0]<<' '<<p[1]<<' '<<p[2]<<std::endl;
		return true;
	}
	else if (isovalue < value1 && isovalue > value2)
	{

		p[0] = float(p2[0] + (p1[0] - p2[0]) * (isovalue - value2)/ (value1 - value2)); 
		p[1] = float(p2[1] + (p1[1] - p2[1]) * (isovalue - value2)/ (value1 - value2));
		p[2] = float(p2[2] + (p1[2] - p2[2]) * (isovalue - value2)/ (value1 - value2));
		//		std::cout<<std::endl;
		//		std::cout<<p1[0]<< ' '<<p1[1]<<' '<<p1[2]<<std::endl;
		//		std::cout<<p2[0]<< ' '<<p2[1]<<' '<<p2[2]<<std::endl;
		//		std::cout<<"Cut2: "<<p[0]<<' '<<p[1]<<' '<<p[2]<<std::endl;
		return true;
	}
	return false;
}

void outputoff(std::vector<Mesh::Point> & v_result)
{
	std::cout<<"OFF"<<std::endl;
	std::cout<< v_result.size()<< ' '<<v_result.size() /2 <<' '<<0<<std::endl;
	for (unsigned i = 0; i < v_result.size(); ++i)
	{
		std::cout<<v_result[i][0]<<' '<<v_result[i][1]<<' '<<v_result[i][2]<<std::endl;
	}

	for (unsigned i = 0 ; i < v_result.size() ; i+=2)
	{
		std::cout<<2<<' '<<i<<' '<<i+1<<std::endl;
	}

}

void outputobj(std::vector<Mesh::Point> & v_result)
{
	for (unsigned i = 0; i < v_result.size(); ++i)
	{
		std::cout<<"v "<<v_result[i][0]<<' '<<v_result[i][1]<<' '<<v_result[i][2]<<std::endl;
	}
	for (unsigned i = 0 ; i < v_result.size() ; i+=2)
	{
		std::cout<<"f "<<' '<<i+1<<' '<<i+2<<std::endl;
	}
}
void generate_seperate_mesh_isoline(Mesh & mesh, std::vector<Mesh::Point> & isoline)
{
	unsigned n_vertex = mesh.n_vertices();
	std::cout<<"o model"<<std::endl;
	for (unsigned i = 0; i < n_vertex; ++i)
	{
		std::cout<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
	}
	std::cout<<"s off"<<std::endl;
	for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
	{
		std::cout<<"f ";
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			std::cout<<fv_it.handle().idx() + 1<<' ';
		}
		std::cout<<std::endl;
	}

	std::cout<<"o isoline"<<std::endl;
	for (unsigned i = 0; i < isoline.size(); ++i)
	{
		std::cout<<"v "<<isoline[i]<<std::endl;
	}
	std::cout<<"s off"<<std::endl;
	//unsigned n_face = mesh.n_faces();	
	for (unsigned i = 0; i < isoline.size() ;i+=2)
	{	
		std::cout<<"f "<<n_vertex+ i + 1 <<' '<<n_vertex+ i + 2 << std::endl;
	}
}
void generate_merged_mesh_isoline(Mesh & mesh, std::vector<Mesh::Point> & isoline)
{
	unsigned n_vertex = mesh.n_vertices();
	
	for (unsigned i = 0; i < n_vertex; ++i)
	{
		std::cout<<"v "<<mesh.point(mesh.vertex_handle(i))<<std::endl;
	}
	
	for (unsigned i = 0; i < isoline.size(); ++i)
	{
		std::cout<<"v "<<isoline[i]<<std::endl;
	}
	
	for (Mesh::FaceIter f_it= mesh.faces_begin(); f_it!=mesh.faces_end(); ++f_it) 
	{
		std::cout<<"f ";
		for (Mesh::FaceVertexIter fv_it = mesh.fv_iter(f_it); fv_it; ++fv_it)
		{
			std::cout<<fv_it.handle().idx() + 1<<' ';
		}
		std::cout<<std::endl;
	}
	//unsigned n_face = mesh.n_faces();	
	for (unsigned i = 0; i < isoline.size() ;i+=2)
	{	
		std::cout<<"f "<<n_vertex+ i + 1 <<' '<<n_vertex+ i + 2 << std::endl;
	}
}